(load "/home/lyouhei/prog/euslib/jsk/voxel_vote.l")

;;(send *tmp-3dp* :nfilter :key #'(lambda (x) (not (isnan (elt x 0)))))
;;(list *plst* *hplst* *first-person* *prev-hlst* *tmp-3dp*)

;;(send *pickview* :viewer :viewsurface :bg-color #f(0.8 0.8 0.9))
;;(send *pickview* :viewtarget (float-vector 860 0 1000))
;;(send *pickview* :viewangle '(11000.0 171.0 18.5))
#|
(progn
  (send *pickview* :viewtarget (float-vector 1965 125 950))
  (send *pickview* :viewangle '(11000.0 111.0 90.0))
  (send *pickview* :viewer :viewing :view-angle 0.5)
  (send *pickview* :viewer :viewing :view-angle 0.5)
  )
(progn
  (send *pickview* :viewtarget (float-vector 1300 0 950))
  (send *pickview* :viewangle '(11000.0 164.5 20.5))
  (send *pickview* :viewer :viewing :view-angle 0.5)
  (send *pickview* :viewer :viewing :view-angle 0.5)
  )
(progn
  (send *pickview* :viewtarget (float-vector 994 -260 350))
  (send *pickview* :viewangle '(11000.0 164.5 20.5))
  (send *pickview* :viewer :viewing :view-angle 0.5)
  (send *pickview* :viewer :viewing :view-angle 0.5)
  )
(progn
  (send *pickview* :viewtarget (float-vector 470 280 670))
  (send *pickview* :viewangle '(11000.0 43.5 20.0))
  (send *pickview* :viewer :viewing :view-angle 0.5)
  (send *pickview* :viewer :viewing :view-angle 0.5)
  )
|#

(defun make-view (data)
  (let ((plst (elt data 0))
        (hplst (elt data 1))
        (fppos (elt data 2))
        (prev-h (elt data 3))
        (3dp (elt data 4))
        objs
        )
    (setq pp (send 3dp :transform-points *base-cds* :create t))
    (send pp :nfilter :key #'(lambda (x) (> 2900 (elt x 0) 1700)))

    (setq fppos (mapcar #'(lambda (x) (send *base-cds* :transform-vector x)) fppos))
    (setq cl0 (make-cylinder 250.0 (elt (elt fppos 0) 2)))
    (send cl0 :translate (float-vector (elt (elt fppos 2) 0)
                                       (elt (elt fppos 2) 1) 0))
    (setf (get cl0 :face-color) :blue)
    (push cl0 objs)
    ;;(v- (elt fppos 0) (elt fppos 2))

    (if (car prev-h)
        (push (let ((c (make-icosahedron 100)))
                (send c :translate (send *base-cds* :transform-vector (car prev-h)))
                (setf (get c :face-color) :yellow) c) objs))
    (if (cdr prev-h)
        (push (let ((c (make-icosahedron 100)))
                (send c :translate (send *base-cds* :transform-vector (cdr prev-h)))
                (setf (get c :face-color) :yellow) c) objs))
    (dolist (o objs)
      (gl::transparent o 0.5))

    (push pp objs)
    objs
    ))

;; remove human
;; remove floor
;; ( segmentation )
;; add-normal -> (send aa :add-normal :cam-coords (get aa :view-coords) :e-threshold 0.2 :k 64)
;; ()
;; ;; plane
(defun make-plane-from-lmeds (ret)
  (let ((nm (car ret))
        (dt (cadr ret))
        pl)
    (setq pl (instance plane :init nm (scale (- dt) nm)))
    pl
    ))

(defun moge (3dp &optional (dist 35) (num-thre 1500))
  (let (pl-lst
        pl plp
        (aa (instance 3dpointcloud :init)))
    (send aa :copy-from 3dp)
    (while t
      (setq pl (make-plane-from-lmeds (lmeds-mat (send aa :matrix) :num 4 :err-rate 0.6)))

      (setq plp (send aa :nfilter :key #'(lambda (x) (< (abs (send pl :distance x)) dist)) :create t))

      (pprint (list (send aa :size) (send plp :size)))

      (if (< (send plp :size) num-thre)
          (return))

      (setf (get plp :plane) pl)

      (push plp pl-lst)
      (send aa :nfilter :key #'(lambda (x) (>= (abs (send pl :distance x)) dist)))
      )
    pl-lst))
;; (send cc :nfilter :key #'(lambda (x) (> (elt x 2) 50.0)))
;; (send cc :nfilter :key #'(lambda (x) (setf (elt x 2) 0) (> (distance x #f(2219 641 0)) 500)))
;; (send cc :nfilter :key #'(lambda (x) (>= (abs (send pl :distance x)) 35)))

(defun cross-line (p1 p2)
  (let ((n1 (send p1 :normal))
        (n2 (send p2 :normal))
        mm mat imat q)
    (setq mm (normalize-vector (v* n1 n2)))

    (setq mat (matrix n1 n2 mm))
    (setq imat (inverse-matrix mat))

    (setq q (transform imat (float-vector (- (p1 . distance))
                                          (- (p2 . distance))
                                          0)))
    (list mm q)))

;; #s(coordinates plist nil rot #2f((0.999941 0.00021 -0.010905) (0.00021 0.999261 0.038445) (0.010905 -0.038445 0.999201)) pos #f(2012.06 -309.758 834.516))

#|
(setq qqq (with-open-file (f "door-plst0.l") (read f)))
(setq apt (elt qqq 0))
(setq bpt (elt qqq 1))

(setq aa (instance 3dpointcloud :init))
(send aa :copy-from apt)
(setq bb (instance 3dpointcloud :init))
(send bb :copy-from bpt)

(setq alst (moge apt))
(setq blst (moge bpt))
|#

;; bbsend bb :nfilter :key #'(lambda (x) (> (elt x 1) -300))
;; send bb :nfilter :key #'(lambda (x) (< (elt x 0) 2500))
;;(setq ppl (make-plane-from-lmeds (lmeds-mat (send bb :matrix) :num 12 :err-rate 0.3)))
;;(send bb :nfilter :key #'(lambda (x) (< (abs (send ppl :distance x)) 35)))


(defun aligned-box (3dp n)
  (let ((rad (atan2 (elt n 1) (elt n 0)))
        (cds (make-coords)) pt
        cb)
    (send cds :rotate rad :-z)
    (setq pt (send 3dp :transform-points cds :create t))

    (send cds :reset-coords)
    (send cds :rotate rad :z)
    (setq cb (make-cube-from-3dp pt cds))
    cb
    ))

#|
(setq lst (with-open-file (f "hoge-pts.l") (read f)))
(setq bdoor (car (last lst)))
(setq bbase (send (elt lst 0) :append-list (list (elt lst 1)) :create t))

(setq p (instance points-model :init))

(setq na (car (lmeds-mat (send bbase :matrix) :num 4 :err-rate 0.6)))
(setq a (aligned-box bbase na))
(setf (get a :point) bbase)

(setq bb (instance 3dpointcloud :init))

(send bb :copy-from bdoor)
(send bb :transform-points (send cds :inverse-transformation))
(setq nb (car (lmeds-mat (send bb :matrix) :num 5 :err-rate 0.6)))
(setq b (aligned-box bb nb))
(send b :transform cds)
(setf (get b :points) bb)

(send p :add-joint b a :rotational)

(progn
  (send (car (send b :bodies)) :draw-on :flush nil :color #f(0 1 0) :width 2)
  (send (car (send a :bodies)) :draw-on :flush nil :color #f(1 1 0) :width 2)
  (send cyl :draw-on :flush t :color #f(1 0 1) :width 2)
  (send hdl :draw-on :flush t :color #f(1 0 0) :width 2)
  (send jnt :draw-on :flush t :color #f(0 0 1) :width 2)
  )

(send p :angle-vector #f(-30))
(send p :update-points)
|#
